#ifndef INS_STATUS_H
#define INS_STATUS_H

#pragma once

#include "INS570D_Define.h"
#include <iostream>
#include <values.h>

enum InsDriver_type{M2,ins570D};

struct INS_STATUS{
    INS_STATUS(){}
    bool updated=false;
    uint8_t header_1=0;
    uint8_t header_2=0;
    uint8_t header_3=0;
    double roll=0;
    double pitch=0;
    double yaw=0;
    double gyro_x=0;
    double gyro_y=0;
    double gyro_z=0;
    double acc_x=0;
    double acc_y=0;
    double acc_z=0;
    double latitude=0;
    double longitude=0;
    double altitude=0;
    double vel_n=0;
    double vel_e=0;
    double vel_d=0;
    double vel_u=0;

    double qx=0,qy=0,qz=0,qw=1;
    aligen_status ali=NOT_ALIGEN;

    uint8_t fix_type=0;
    uint8_t satellites_num=0;
    uint8_t wheel_speed_status=0;
    data_type data_type_;
    double timestamp=0;
    //数据标准差
    double lat_std=DBL_MAX;
    double lon_std=DBL_MAX;
    double alti_std=DBL_MAX;
    double vn_std=DBL_MAX;
    double ve_std=DBL_MAX;
    double vd_std=DBL_MAX;
    double vu_std=DBL_MAX;
    double roll_std=DBL_MAX;
    double pitch_std=DBL_MAX;
    double yaw_std=DBL_MAX;
    // 设备温度
    double dev_temperature=0;

    void debugString(){
        printf("=======================================================\n");
        printf("roll [%f] \t pitch [%f] \t yaw [%f] \n",roll,pitch,yaw);
        printf("acc_x [%f] \t acc_y [%f] \t acc_z [%f] \n",acc_x,acc_y,acc_z);
        printf("gyro_x [%f] \t gyro_y [%f] \t gyro_z [%f] \n",gyro_x,gyro_y,gyro_z);
        printf("latitude [%f] \t longitude [%f] \t altitude [%f] \n",latitude,longitude,altitude);
        printf("vel_n [%f] \t vel_e [%f] \t vel_d [%f] \n",vel_n,vel_e,vel_d);

//        printf("lat_std [%f] \t lon_std [%f] \t alti_std [%f] \n",lat_std,lon_std,alti_std);
//        printf("vn_std [%f] \t ve_std [%f] \t vd_std [%f] \n",vn_std,ve_std,vd_std);
//        printf("roll_std [%f] \t pitch_std [%f] \t yaw_std [%f] \n",roll_std,pitch_std,yaw_std);
        printf("wheel speed status [%x] \n",wheel_speed_status);
        printf("satellites_num: [%d] \n",satellites_num);
        printf("gps Status: [%x] \n",fix_type);
    }
};

struct RTK_STATUS{
    RTK_STATUS(){}
    bool updated=false;
    double latitude=0;
    double longitude=0;
    double altitude=0;
    uint8_t fix_type=0;
    uint8_t satellites_num=0;
    //Timestamp
    double timeStamp=0;
    //HDOP
    double hdop=0;
    //数据标准差
    double lat_std=DBL_MAX;
    double lon_std=DBL_MAX;
    double alti_std=DBL_MAX;

    void debugString(){
        printf("=======================================================\n");
        printf("latitude [%f] \t longitude [%f] \t altitude [%f] \n",latitude,longitude,altitude);

//        printf("lat_std [%f] \t lon_std [%f] \t alti_std [%f] \n",lat_std,lon_std,alti_std);
//        printf("vn_std [%f] \t ve_std [%f] \t vd_std [%f] \n",vn_std,ve_std,vd_std);
//        printf("roll_std [%f] \t pitch_std [%f] \t yaw_std [%f] \n",roll_std,pitch_std,yaw_std);
        printf("satellites_num: [%d] \n",satellites_num);
        printf("gps Status: [%x] \n",fix_type);
    }
};

#endif // INS_STATUS_H
